/** Implementation of the meta-heuristics used.
 *
 * @author Carlos Colmenares (06-39380)
 * @author Kelwin Fernández (07-40888)
 */

#include "meta_heuristics.h"
#include <utility>
#include <tr1/unordered_set>
#include <set>
#include <queue>
#include <vector>
#include <string>
#include <cstring>
#include <stack>
#include <cmath>

//Taboo Search Macros
#define TS_MEMORY_SIZE 150
#define TS_ELITES_SIZE 80
#define TS_ELITES_ITER 200

//Ant Clust Algorithm Macros
#define ACA_FREE -1
#define PI 3.1415926

//Simulated Annealing Macros
//#define SA_Max_Ap 0.6
//#define SA_p_T 7.0
//#define SA_MAX_K 500
//#define SA_ALPHA 0.85
//#define SA_RO 1.04
#define SA_Max_Ap 0.7
#define SA_p_T 10
#define SA_MAX_K 400
#define SA_ALPHA 0.95
#define SA_RO 1.01

using namespace std;

extern global_data g_data;

void global_data::initialize(){

    problem_instance =
        new clustering_instance(string(data_input_file)); 
    
    //Set the criterion
    if( strcmp(g_data.criterion,"Davies-Bouldin") == 0 ){
        state::criterion_id = 0;        
    }
    else if( strcmp(g_data.criterion,"Dunn") == 0 ){
        state::criterion_id = 1;        
    }    
}

/**
 * This procedudre receives a state, calculates its whole
 * neighborhood, and chooses the neighbor with the best
 * quality criterion. The state::acceptance_criteria procedure
 * is used for comparisons.
 * @param s The state which neighborhood will be calculated
 * @return The best neighbor in the state's neighborhood
 */
state* best_neighborhood(state *s){
    
    state* best_neighbor = NULL;
    //Calculate all possible neighbors    
    for (int c = 0; c < s->num_clusters; c++){
        for (int a = 0; a < s->prob->n_attributes; a++){
            state* neighbor = s->get_neighbor(c, a);
            //Choose the best
            if ( !best_neighbor ||
                state::acceptance_criteria(best_neighbor,neighbor) ){
            
                delete best_neighbor;
                best_neighbor = neighbor;
            
            } else {
                delete neighbor;
            }
        }
    }
    
    return best_neighbor;
}

/**
 * This procedudre receives a state, calculates its whole
 * neighborhood, and chooses the neighbor with the best
 * quality criterion. The state::acceptance_criteria procedure
 * is used for comparisons. However, before calculating
 * the neighborhood, the state's centroids are moved to their
 * geometrical center first.
 * @param s The state which neighborhood will be calculated
 * @return The best neighbor in the state's neighborhood
 */
state* best_smooth_neighborhood(state *s){
    
    state* best_neighbor = NULL;
    
    for (int c = 0; c < s->num_clusters; c++){
        for (int a = 0; a < s->prob->n_attributes; a++){
            state* neighbor = s->get_smooth_neighbor(c, a);
            
            if ( !best_neighbor ||
                state::acceptance_criteria(best_neighbor,neighbor) ){
                
                delete best_neighbor;
                best_neighbor = neighbor;
                
            } else {
                delete neighbor;
            }
        }
    }
    
    return best_neighbor;
}

state* k_means(state* initial_state){
    
    state* current = initial_state;
    
    if ( !initial_state ){
        current = new state(g_data.problem_instance, g_data.num_clusters);
    } else {
        current = new state(initial_state);
    }
    
    state* best = current;
    
    int iter = 0;
    bool cambio = true;
    while ( iter++ < g_data.max_iterations && cambio){
        state* neighbor = new state(current);
        neighbor->assign_nearest();
        neighbor->recompute_centroids();
        
        if ( current->value != neighbor->value){
            cambio = true;
        } else {
            cambio = false;
        }
        
        delete current;
        current = neighbor;
        
        if ( state::acceptance_criteria( best, current ) ){
            delete best;
            best = new state(current);
        }
    }
    delete current;
    
    return best;
}

state* local_search(state* initial_state){
    
    state* current;
    
    if ( initial_state ){
        current = new state(initial_state);    
    } else {
        current = new state(g_data.problem_instance, g_data.num_clusters);
    }
    
    state* neighbor;
    
    int iter = 0;
    while ( iter++ < g_data.max_iterations ){
        
        neighbor = best_neighborhood(current);
        
        if ( neighbor == NULL ){
            break;
        }
        
        if( state::acceptance_criteria(current, neighbor) ){
            delete current;
            current = neighbor;
        } else {
            delete neighbor;
            break;
        }
    }
    g_data.num_iterations = iter;
    
    return current;
}

state* iterated_local_search(state* initial_state){
    
    state *current, *last, *best, *tmp;
    
    if ( !initial_state ){
        current = new state(g_data.problem_instance, g_data.num_clusters);
    }
    else {
        current = new state(initial_state);
    }
    
    tmp = local_search(current);
    delete current;
    
    best = tmp;
    
    
    last = best;
    
    int iter = 0;
    while( iter++ < g_data.max_iterations ){
        
        current = last->disturb();
        if ( best != last ){
            delete last;
        }
        
        state* current_best = local_search( current );
        
        if ( current != best and current != current_best ){
            delete current;
        } else {
            delete current_best;
            current_best = current;
        }
        
        last = current_best;
        
        if ( state::acceptance_criteria(best,current_best) ){
            delete best;
            best = current_best;
        }
    }
    g_data.num_iterations = iter-1;
    
    return best;
}


state* taboo_search_local_search(state* initial_state = NULL){
   
    state* current;
    
    if ( !initial_state ){
        current =
            new state(g_data.problem_instance, g_data.num_clusters);
    }
    else {
        current = new state(initial_state);
    }

    //When the algorithm makes diversification,
    //this variable keeps track of the best
    //solution among all diversifications.
    state* global_best = NULL;
    

    //The taboo list will be a hash table, if
    //the pair <i,j>< belongs to it, then aproaching
    //centroid "i" to point "j" is taboo.
    set< pair<int,int> > taboo_list; 

    //The next queue is created for being able
    //of erasing the "old" taboo states in the taboo
    //list
    queue< pair<int,int> > memory_q;

    int iter = 0;
    while( iter++ < g_data.max_iterations ){        

        //Get the best neighbor that is not in
        //the taboo list

        state* best_neighbor = NULL;
        pair<int,int> bn_pair;

        for (int c = 0; c < g_data.num_clusters; c++){
            for (   int a = 0; 
                    a < g_data.problem_instance->n_attributes;
                    a++
                )
            {
                //Check the taboo
                if( taboo_list.count( make_pair(c,a) ) > 0 )
                    continue;
                
                state* neighbor = 
                    current->get_neighbor(c, a);
                
                if (    !best_neighbor ||
                        state::acceptance_criteria(best_neighbor,
                                                    neighbor)
                    )
                {
                    delete best_neighbor;
                    best_neighbor = neighbor;
                    bn_pair = make_pair(c,a);
                } else {
                    delete neighbor;
                }
            }
        }
        
        //Check if the best neighbor is better than the
        //current value
        if( !best_neighbor ||
            !state::acceptance_criteria(current,best_neighbor) )
        { //No possible neighbors or algorithm's end
            
            //Actualization of the global best
            if ( !global_best || 
                 state::acceptance_criteria(global_best,current)
                )
            {
                delete global_best;
                global_best = current;
            }
            
            //Make diversification here
            break;
        }
        
        //Make the neighbor taboo
        memory_q.push( bn_pair );
        
        //Erase old entries
        if( (int)memory_q.size() > TS_MEMORY_SIZE ){
            taboo_list.erase( memory_q.front() );
            memory_q.pop();
        }
        
        //Repeat for the new state
        delete current;
        current = best_neighbor;
    }

    //Make actualization of the global_best
    if ( !global_best ||
          state::acceptance_criteria(global_best,current)
       )
    {
        global_best = current;
    }
    g_data.num_iterations = iter;
    
    //cout << "Solution got: " << endl;
    return global_best;
}

state* taboo_search(state* initial_state) {
   
    cout << "Calculating Elites" <<endl;
    //Calculate the initial elites. They will be stored in
    //a list, because they will be tryed to be
    //improved afterwards
    queue< state* > elite_list;

    state* best_elite = NULL;

    for(int i=0; i<TS_ELITES_SIZE ; i++){

        state* elite = taboo_search_local_search();
        elite_list.push( elite );
        
        elite->print();
        
        if( !best_elite ||
            state::acceptance_criteria(best_elite,elite)
          ){
            best_elite = elite;
        }
        //This is necessary in case there was an initial state
        initial_state = NULL;
        //sleep(1);
    }

    //Now, for a fixed number of iterations, try to
    //make them better, in case they don't, discard them
    cout << ".......Improving Elites......." << endl;
    for(int i=0; i<g_data.max_iterations && !elite_list.empty() ; i++){
        
        state* better_sol =
            taboo_search_local_search(elite_list.front());

        cout << ">> "<<elite_list.front()->value<<" >> "
            <<better_sol->value;
        if(elite_list.front()->value < better_sol->value )
             cout << "(+)"<<endl;
        else if(elite_list.front()->value > better_sol->value ) 
             cout << "(-)"<<endl;
        else
             cout << "(=)"<<endl;

        //Check if it improoved
        if( state::acceptance_criteria(elite_list.front(),better_sol) ){
            
            if( state::acceptance_criteria(best_elite,better_sol) )
                best_elite = better_sol;

            elite_list.push(better_sol);
            
            //better_sol->print();
        }

        elite_list.pop();
        
        g_data.num_iterations = i;
    }
    g_data.num_iterations += TS_ELITES_SIZE;
    
    //clean elite list! 
    return best_elite;
}

state* variable_neighborhood_search(state* initial_state){
    state* current;
    
    if ( !initial_state ){
        current = new state(g_data.problem_instance, g_data.num_clusters);
    } else {
        current = new state(initial_state);    
    }
    
    state* neighbor;
    
    int iter = 0;
    state* (*operators[])(state*) = {&best_smooth_neighborhood,
                                     &best_neighborhood};
    int num_operators = 2;
    
    while ( iter++ < g_data.max_iterations ){
        for (int k=0; k<num_operators; k++){
            neighbor = (operators[k])(current);
            if ( neighbor == NULL ){
                continue;
            }
            
            if( state::acceptance_criteria(current, neighbor) ){
                break;
            } else {
                delete neighbor;
                neighbor = NULL;
            }
            
        }
        
        if ( neighbor == NULL ){
            break;
        }
        
        //delete current;
        current = neighbor;
    }
    g_data.num_iterations = iter;
    
    return current;
}

double ACA_f(int i, int k, vector<int> cell[], int n_data, double alpha){
    
    double sol = 0;
    double alpha2 = alpha*alpha;
    int nk = cell[k].size();
    
    for (int j=0; j < nk; j++){
        sol += alpha2 / ( alpha2 
                          + euclidean_distance(
                                g_data.problem_instance->n_attributes,
                                g_data.problem_instance->data[i],
                                g_data.problem_instance->data[cell[k][j]]
                                )
                        );
    }
    
    return sol / nk;
}

double ACA_compute_alpha(){
    double alpha = 0.0;
    int n = g_data.problem_instance->n_data;
    int n_at = g_data.problem_instance->n_attributes;
    
    for (int i=0; i<n; i++){
        for (int j=0; j<n; j++){
            alpha += euclidean_distance( n_at,
                                         g_data.problem_instance->data[i],
                                         g_data.problem_instance->data[j]
                                       );
        }
    }
    
    return alpha / ( n * (n-1) );
}

double ACA_prob_pick(int i, int k, vector<int> cell[], double f){
    if ( cell[k].size() == 1 ){
        return 1.0;
    } else if ( cell[k].size() == 2 ){
        return g_data.aca_q;
    }
    
    double sol = cos( PI/2.0 * f );
    return sol*sol;
}

double ACA_prob_drop(int i, int k, double f){
    double sol = sin(PI/2*f);
    return sol*sol;
}

state* ant_clust_algorithm(){
    
    //1 Initialization phase
    int n_data = g_data.problem_instance->n_data;
    vector<int> cell[n_data];
    int num_ants = min(g_data.num_ants, n_data/2);
    
    int ants[num_ants];
    vector<int> free_data;
    int data_cell[n_data];
    int data_pos[n_data];
    double alpha = ACA_compute_alpha();
    double R = 0.0;
    
    int random_selected_ants[num_ants];
    
    srand((unsigned)time(0));
    
    //1.1 Place each data in a cell
    for (int i=0; i<n_data; i++){
        cell[i].push_back(i);
        
        free_data.push_back(i);
        data_cell[i] = i;
        data_pos[i] = 0;
    }
    
    //1.2 Each ant pick up a randomly chosen data
    for (int i=0; i<num_ants; i++){
        
        int random_data = rand() % ( n_data -i );
        int data = free_data[random_data];
        int cell_index = data_cell[data];
        
        ants[i] = data;
        
        // Pick up the data from the cell
        cell[cell_index][data_pos[data]] =
            cell[ cell_index ][ cell[cell_index].size()-1 ];
        cell[cell_index].pop_back();
        
        // Mark data as unfree
        free_data[random_data] = free_data[free_data.size()-1];
        free_data.pop_back();
        
        random_selected_ants[i] = i;
    }
    
    //2 Main loop
    int iter = 0;
    while( iter++ < g_data.max_iterations ){
        
        //print_free_data(free_data);
        
        for (int a=0; a<num_ants; a++){
            
            // Randomly selected from all ants
            int random_index = rand() % ( num_ants - a);
            int ant_selected = random_selected_ants[random_index];
            
            random_selected_ants[random_index] = random_selected_ants[num_ants-a-1];
            random_selected_ants[num_ants - a - 1] = ant_selected;
            
            if( ants[ant_selected] != ACA_FREE ){
                // ant is carrying data ants[ant_selected]
                
                int k = rand() % n_data;
                int data_drop = ants[ant_selected];
                
                double f = ACA_f(data_drop, k,
                                 cell, n_data, alpha);
                
                double p_drop = ACA_prob_drop(data_drop, k, f);
                
                R = rand() / RAND_MAX;
                
                if ( R <= p_drop ){
                    // Move ant_selected to the cell k and drop
                    // the data ant[ant_selected]
                    
                    cell[k].push_back(data_drop);
                    
                    free_data.push_back(data_drop);
                    data_cell[data_drop] = k;
                    data_pos[data_drop] = cell[ data_cell[data_drop] ].size()-1;
                    
                    ants[ant_selected] = ACA_FREE;
                    
                    //cout << "ant " << ant_selected << " drops " << data_drop << " in cell " << k << endl;
                }
                
            } else {
                // ant_selected is free
                
                random_index = rand() % free_data.size();
                
                int data_index = free_data[random_index];
                
                double f = ACA_f(data_index, data_cell[data_index],
                                 cell, n_data, alpha);
                
                double p_pick = ACA_prob_pick(data_index,
                                              data_cell[data_index],
                                              cell,
                                              f
                                             );
                
                R = rand() / RAND_MAX;
                if ( R <= p_pick ){
                    
                    // Move the and to the cell data_cell[data_index] and pick
                    // up the data data_index
                    
                    // Pick up the data from the cell
                    int cell_index = data_cell[ data_index ];
                    
                    cell[cell_index][ data_pos[data_index] ] =
                        cell[ cell_index ][ cell[cell_index].size()-1 ];
                    cell[cell_index].pop_back();
                    
                    // Mark data as unfree
                    free_data[random_index] = free_data[free_data.size()-1];
                    free_data.pop_back();
                    
                    ants[ ant_selected ] = data_index;
                    
                    //cout << "ant " << ant_selected << " picks" << " " << data_index << " from cell " << cell_index << endl;
                }
            }
        }
    }
    
    //3 Transform the current representation to a centroids representation
    state* s = new state(g_data.problem_instance, cell);
    int it = 0;
    while( true ){
        int nc = s->num_clusters;
        cout << it++ << " " << nc << " " << s->value << endl;
        
        s->filter_unused_clusters();
        s = k_means(s);
        
        if ( nc == s->num_clusters ){
            break;
        }
    }
    
    return s;
}

state* ant_clust_hybrid_algorithm(){
    
    /* Constructive part */
    state* s = ant_clust_algorithm();
    
    cout << "ACA done --> " << s->value << endl;
    
    /* Hybrid part: ILS + kmeans */
    g_data.max_iterations = 5000;
    s = iterated_local_search(s);
    
    cout << "ILS done --> " << s->value << endl;
    
    s = k_means(s);
    
    cout << "KMEANS done --> " << s->value << endl;
    
    int it = 0;
    while( true ){
        int nc = s->num_clusters;
        cout << it++ << " " << nc << " " << s->value << endl;
        
        s->filter_unused_clusters();
        s = k_means(s);
        
        if ( nc == s->num_clusters ){
            break;
        }
    }
    return s;
}

state* meta_ant_clust_algorithm(){
    
    state * s = ant_clust_algorithm();
    
    while( s->num_clusters > g_data.num_clusters ){
        
        clustering_instance* tmp = g_data.problem_instance;
        
        clustering_instance* centroids_instance = 
            new clustering_instance(s->prob->n_attributes,
                                    s->num_clusters,
                                    s->centroids);
        
        g_data.problem_instance = centroids_instance;
        
        state *new_s = local_search();
        
        s->filter_unused_clusters();
        s = k_means(s);
        
        for (int c=0; c<new_s->num_clusters; c++){
            for (int a=0; a<s->prob->n_attributes; a++){
                s->centroids[c][a] = new_s->centroids[c][a];
            }
        }
        s->num_clusters = new_s->num_clusters;
        
        free( new_s );
        //free( centroids_instance);
        g_data.problem_instance = tmp;
    }
    
    return s;
}


void individual::initialize(){
    int n_data = g_data.problem_instance->n_data;
    int n_attributes = g_data.problem_instance->n_attributes;
    
    genotype = (double**)malloc( n_data*sizeof(double*) );
    for (int i=0; i < n_data; i++){
        genotype[i] = (double*)malloc( n_attributes*sizeof(double) );
    }
}

state** generate_initial_population(int n = 200){
    
    state** population;
    int* randomized_sequence;
    int n_data;
    
    // Initialize the population and the sequence of random data selected
    
    n_data = g_data.problem_instance->n_data;
    
    population = (state**)malloc(n*sizeof(state*));
    for (int i=0; i<n; i++){
        population[i] = new state(g_data.problem_instance, g_data.num_clusters);
    }
    
    randomized_sequence = (int*)malloc(n_data*sizeof(int));
    for (int i=0; i<n_data; i++){
        randomized_sequence[i] = i;
    }
    
    int counter = 0;
    srand((unsigned)time(0));
    
    // For each individual in the population
    for (int i=0; i<n; i++){
        
        // For each individual's centroid
        for (int c=0; c<g_data.num_clusters; c++){
            
            int random_data = rand() % ( n_data );//- counter );
            int index = randomized_sequence[random_data];
            
            for (int a=0; a<g_data.problem_instance->n_attributes; a++){
                population[i]->centroids[c][a] = 
                    g_data.problem_instance->data[index][a];
            }
            /*
            randomized_sequence[random_data] = 
            randomized_sequence[n_data-1-counter];
            randomized_sequence[n_data-1-counter] = index;
            */
            counter++;
            if ( counter == n_data ){
                counter = 0;
            }
        }
    }
    
    for (int i=0; i<g_data.population_size; i++){
        population[i]->calc_criterion();
    }
    
    sort( population, population + g_data.population_size,
          state::acceptance_criteria);
    
    free( randomized_sequence );
    
    return population;
}

void select_parents( state** population, vector< pair<int,int> >& couples ){
    
    // Compute the maximum fitness and the sum of the fitness values.
    double max_fitness = 0.0;
    double sum_fitness = 0.0;
    
    for (int i=0; i<g_data.population_size; i++){
        max_fitness = max(max_fitness, population[i]->value);
        sum_fitness += population[i]->value;
    }
    
    for (int i=0; i<g_data.num_couples; i++){
        
        pair<int, int> parents;
        
        for (int p = 0; p < 2; p++ ){
            
            double random = rand()*1.0 / RAND_MAX;
            double acum = 0;
            
            acum = 0;
            for (int j=0; j<g_data.population_size; j++){
                if ( acum <= random 
                        && random <= acum + population[
                                              g_data.population_size - j -1
                                              ]->value*1.0 /sum_fitness )
                {
                    if ( p )
                        parents.second = j;
                    else
                        parents.first = j;
                    
                    break;
                }
                acum += population[ g_data.population_size - j -1]->value
                          / sum_fitness;
            }
        }
        
        couples.push_back( parents );
    }
}

void crossover( state** population, vector< pair<int, int> > couples,
                vector< state* >& offsprings, bool hybrid = false )
{
    int genotype_length = 
        g_data.problem_instance->n_attributes*g_data.num_clusters;
    
    // Cross every couple
    while( !couples.empty() ){
        
        pair<int,int> couple = couples[ couples.size() -1 ];
        couples.pop_back();
        
        // offspring1 is: couple.first  - couple.second
        // offspring1 is: couple.second -  couple.first
        state* offspring1 = new state(population[couple.first]);
        state* offspring2 = new state(population[couple.second]);
        
        // Point for crossover
        int centroid_cut = rand() % genotype_length;
        
        for (int c=centroid_cut; c<g_data.num_clusters; c++){
            for (int a=0; a<g_data.problem_instance->n_attributes; a++){
                offspring1->centroids[c][a] =
                    population[couple.second]->centroids[c][a];
                offspring2->centroids[c][a] =
                    population[couple.first]->centroids[c][a];
            }
        }
        
        state* off_tmp;
        
        if ( hybrid ){
            off_tmp = local_search(offspring1);
            if ( state::acceptance_criteria( offspring1, off_tmp )){
                delete offspring1;
                offspring1 = off_tmp;
            } else {
                delete off_tmp;
            }
            
            off_tmp = local_search(offspring2);
            if ( state::acceptance_criteria( offspring2, off_tmp )){
                delete offspring2;
                offspring2 = off_tmp;
            } else {
                delete off_tmp;
            }
        }
        
        offsprings.push_back(offspring1);
        offsprings.push_back(offspring2);
    }
    
}

void mutate( vector<state*> offsprings ){
    
    for (int i=0; (unsigned)i<offsprings.size(); i++){
        if ( rand() * 1.0 / RAND_MAX <= g_data.mutation_prob ){
            int index = rand() % offsprings.size();
            int centroid = rand() % g_data.num_clusters;
            int attribute = rand() % g_data.problem_instance->n_attributes;
            offsprings[ index ]->centroids[ centroid ][ attribute ] = 
                g_data.problem_instance->data
                   [rand() % g_data.problem_instance->n_data]
                   [ rand() % g_data.problem_instance->n_attributes ];
        }
    }
}

void evaluate( vector<state*> offsprings ){
    
    for (int i=0; (unsigned)i<offsprings.size(); i++)
        offsprings[i]->calc_criterion();
}


void replacement( state** population, vector<state*>& offsprings){
    
    // Compute the maximum fitness and the sum of the fitness values.
    double max_fitness = 0.0;
    double sum_fitness = 0.0;
    
    for (int i=0; i<g_data.population_size; i++){
        max_fitness = max(max_fitness, population[i]->value);
        sum_fitness += population[i]->value;
    }
    
    while( !offsprings.empty() ){
        
        double random = rand()*1.0 / RAND_MAX;
        double acum = 0;
        
        acum = 0;
        for (int j=0; j<g_data.population_size; j++){
            
            if ( acum <= random 
                && random <= acum 
                    + population[j]->value*1.0 /sum_fitness )
            {
                delete population[j];
                
                population[j] = 
                    new state(offsprings[offsprings.size()-1]);
                
                while( j > 0 
                        && state::acceptance_criteria( population[j-1],
                                                        population[j]
                                                        )
                        )
                {
                    state* tmp = population[j];
                    population[j] = population[j-1];
                    population[j-1] = tmp;
                    j--;
                }
                
                break;
            }
            
            acum += population[j]->value/ sum_fitness;
        }
        
        delete offsprings[offsprings.size() -1];
        offsprings.pop_back();
    }
}

void restart_population( state** population, vector<state*> offsprings){
    
    for (int i=0; i<(1-g_data.prop_restart)*g_data.population_size; i++){
        offsprings.push_back( 
            new state(g_data.problem_instance, g_data.num_clusters) );
    }
    replacement( population, offsprings );
}


state* genetic_algorithm(bool hybrid){
    
    int iter = 0;
    vector< pair<int,int> > couples;
    state** population;
    vector< state* > offsprings;
    state* best_solution = NULL;
    int last_update = 0;
    
    srand((unsigned)time(0));
    
    // Choose the initial population of individuals
    population = generate_initial_population(g_data.population_size);
    
    while ( iter++ < g_data.max_iterations){
        select_parents( population, couples );
        crossover( population, couples, offsprings, hybrid );
        mutate( offsprings );
        evaluate( offsprings );
        replacement( population, offsprings);
        
        if ( !best_solution 
              || state::acceptance_criteria(best_solution, population[0])
           )
        {
            
            if ( best_solution ){
                delete best_solution;
            }
            
            best_solution = new state(population[0]);
            
            cout << "best individual (iter " << iter << "): "
                 << best_solution->value << endl;
            
            last_update = iter;
        }
        
        if ( iter - last_update >= g_data.prop_restart*g_data.max_iterations ){
            
            last_update = iter;
            cout << "RESTART ("
                 << iter << "/" << g_data.max_iterations << ")"
                 <<  endl;
            
            restart_population( population, offsprings);
        }
    }
    
    for (int i=0; i<g_data.population_size; i++){
        delete population[i];
    }
    free(population);
    
    return best_solution;
}

state* hybrid_genetic_algorithm(){
    g_data.max_iterations = 50;
    g_data.prop_restart = 1.0;
    return genetic_algorithm(false);
}

void select_parents_alpha( state** population,
                           vector< pair<int,int> >& couples,
                           state* alpha_male
                         )
{
    // Compute the maximum fitness and the sum of the fitness values.
    double max_fitness = 0.0;
    double sum_fitness = 0.0;
    
    for (int i=0; i<g_data.population_size; i++){
        max_fitness = max(max_fitness, population[i]->value);
        sum_fitness += population[i]->value;
    }
    
    int alpha_male_index = 0;
    for (int i=0; i<g_data.population_size; i++){
        if ( population[i] == alpha_male ){
            alpha_male_index = i;
            break;
        }
    }
    
    for (int i=0; i<g_data.num_couples; i++){
        
        pair<int, int> parents;
        
        double random = rand()*1.0 / RAND_MAX;
        double acum = 0;
        
        parents.first = alpha_male_index;
        
        acum = 0;
        for (int j=0; j<g_data.population_size; j++){
            if ( acum <= random 
                    && random <= acum + population[
                                            g_data.population_size - j -1
                                            ]->value*1.0 /sum_fitness )
            {
                parents.second = j;
                break;
            }
            acum += population[ g_data.population_size - j -1]->value
                        / sum_fitness;
        }
        
        couples.push_back( parents );
    }
}

state* alpha_male_tournament( state** population, state* alpha_male ){
    
    // Compute the maximum fitness and the sum of the fitness values.
    double max_fitness = 0.0;
    double sum_fitness = 0.0;
    vector<state*> tournament;
    
    for (int i=0; i<g_data.population_size; i++){
        max_fitness = max(max_fitness, population[i]->value);
        sum_fitness += population[i]->value;
    }
    
    for (int i=0; i<g_data.tournament_size; i++){
        double random = rand()*1.0 / RAND_MAX;
        double acum = 0;
        
        acum = 0;
        for (int j=0; j<g_data.population_size; j++){
            if ( acum <= random 
                    && random <= acum + population[
                                            g_data.population_size - j -1
                                            ]->value*1.0 /sum_fitness )
            {
                tournament.push_back( population[j] );
            }
            acum += population[ g_data.population_size - j -1]->value
                        / sum_fitness;
        }
    }
    tournament.push_back( alpha_male );
    
    max_fitness = 0.0;
    sum_fitness = 0.0;
    
    for (int i=0; (unsigned)i<tournament.size(); i++){
        max_fitness = max(max_fitness, tournament[i]->value);
        sum_fitness += tournament[i]->value;
    }
    
    double random = rand()*1.0 / RAND_MAX;
    double acum = 0;
    
    acum = 0;
    for (int j=0; (unsigned)j<tournament.size(); j++){
        if ( acum <= random 
                && random <= acum + population[
                                        tournament.size() - j -1
                                        ]->value*1.0 /sum_fitness )
        {
            return tournament[j];
        }
        acum += population[ tournament.size() - j -1]->value
                    / sum_fitness;
    }
    
    return alpha_male;
}

state* alpha_male_genetic_algorithm(){
    int iter = 0;
    vector< pair<int,int> > couples;
    state** population;
    vector< state* > offsprings;
    state* best_solution = NULL;
    int last_update = 0;
    state* alpha_male;
    
    g_data.max_iterations = 50;
    
    srand((unsigned)time(0));
    
    // Choose the initial population of individuals
    population = generate_initial_population(g_data.population_size);
    alpha_male = population[0];
    
    while ( iter++ < g_data.max_iterations){
        select_parents_alpha( population, couples, alpha_male );
        crossover( population, couples, offsprings );
        mutate( offsprings );
        evaluate( offsprings );
        replacement( population, offsprings);
        alpha_male = alpha_male_tournament( population, alpha_male );
        
        if ( !best_solution 
              || state::acceptance_criteria(best_solution,population[0])
           )
        {
            if ( best_solution ){
                delete best_solution;
            }
            
            best_solution = new state(population[0]);
            cout << "best individual (iter " << iter << "): "
                 << population[0]->value << endl;
            
            last_update = iter;
        }
        
        if ( iter - last_update >= g_data.prop_restart*g_data.max_iterations ){
            
            last_update = iter;
            cout << "RESTART ("
                 << iter << "/" << g_data.max_iterations << ")"
                 <<  endl;
            
            restart_population(population, offsprings);
        }
    }
    for(int i=0; i<g_data.population_size; i++){
        delete population[i];
    }
    free(population);
    
    return best_solution;    
}

state* simulated_annealing(state* initial_state){
     
    state *current, *best, *neigh;
    
    if ( !initial_state ){
        current = new state( g_data.problem_instance,
                g_data.num_clusters);
    }
    else {
        current = new state(initial_state);
    }
    
    best = current;
    
    int sa_K = SA_MAX_K;  //Max iters
    int sa_A = (int) sa_K * SA_Max_Ap;    //Max acceptances
    double temp = current->get_value() * SA_p_T;


    for(int i=0; i< g_data.max_iterations; i++){
        cout << "Iteration: " << i << "\n";
        int acc = 0, nacc = 0, btt = 0;
        int k = 0; //iteration counter
        int a = 0; //"acceptance" counter


        while( k++ < sa_K && a < sa_A ){
            srand( rand() );
            
            //Get a random neighbor
            neigh = current->get_random_neighbor();

            if ( state::acceptance_criteria(current, neigh) ){
                btt++;
                if( current == best ){
                    best = neigh;
                }
                else if( state::acceptance_criteria(best, neigh) ){
                    delete best;
                    best = neigh;
                }
                delete current;
                current = neigh;
                a++;
            }
            else {
                double threshold  = exp(
                    -abs(neigh->get_value()-current->get_value())/temp
                );
                double rnd = rand() / (double(RAND_MAX)+1);

                if(rnd < threshold){
                    acc++;
                    if(current != best)
                        delete current;
                    current = neigh;
                    a++;
                }
                else {
                    delete neigh;
                    nacc++;
                }
            }
        }
        cout << "Better: " << btt << ", Accepted: " << acc
            << ", not Accepted: " << nacc << ". K: " << sa_K <<
            "\n";
        temp *= SA_ALPHA;
        sa_K = int(SA_RO * sa_K);
    }
    return best;
}


